#include "Arduino.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "MultiWii.h"
// cabecalho de declaracao 
#include "RXSwitch.h"
// biblioteca de leitura do receptor
#include <RCSwitch.h>
// convencoes
#define RCINTERRUPT 4 // interrupcao de recepcao pino 19 do Arduino Mega
// instancia do leitor
RCSwitch rcs;

//Variaveis para recebimento de comandos
int parametro1, parametro2;

//Variaveis para teste logicos das posiçoes referentes aos comandos
const int BIT_THROTTLE = B00001000;
const int BIT_YAW = B00000100;
const int BIT_ROLL = B00000001;
const int BIT_PITCH = B00000010;

//parametros de comandos
#define PULSO_LIGAR      256
#define PULSO_DESLIGAR   512

void initRXSwitch() {
  rcs = RCSwitch();
  rcs.enableReceive(RCINTERRUPT);
  
  resetRcData();  
}

void resetRcData() {
  for (int i = 0; i < RC_CHANS; i++) {
    rcData[i] = 1500;  
  }  
}

void controleDrone(int parametro1, int parametro2){
       
    //Testes logicos de posiçoes dos comandos UP
      if(parametro1 & BIT_THROTTLE){
        if(rcData[THROTTLE]<1600){
          rcData[THROTTLE] = rcData[THROTTLE] + 20;
        }
      }
      
      if(parametro1 & BIT_YAW){
        if(rcData[YAW] < 1900){
          rcData[YAW] = rcData[YAW] + 20;
        }
      }
      
      if(parametro1 & BIT_PITCH){
        if(rcData[PITCH] < 1900){
          rcData[PITCH] = rcData[PITCH] + 20;
        }
      }
      
      if(parametro1 & BIT_ROLL){
        if(rcData[ROLL] < 1900){
          rcData[ROLL] = rcData[ROLL] + 20;
        }
      }
      
    //Testes logicos de posiçoes dos comandos DOWN
      if(parametro2 & BIT_THROTTLE){
        if(rcData[THROTTLE] > 1000){
          rcData[THROTTLE] = rcData[THROTTLE] - 20;
        }
      }
      
      if(parametro2 & BIT_YAW){
        if(rcData[YAW] > 1000){
          rcData[YAW] = rcData[YAW] - 20;
        }
      }
      
      if(parametro2 & BIT_ROLL){
        if(rcData[ROLL] > 1000){
          rcData[ROLL] = rcData[ROLL] - 20;
        }
      }
      
      if(parametro2 & BIT_PITCH){
        if(rcData[PITCH] > 1000){
          rcData[PITCH] = rcData[PITCH] - 20;
        }
      }
}

void computeRXSwitch() {
  if (rcs.available()) {
    long pulso = rcs.getReceivedValue();
    
    //teste para identificar comando de ligar
    if(pulso == PULSO_LIGAR){
      rcData[THROTTLE] = 1000;
      rcData[YAW] = 2000;
      //verificar um flag que representa o ativamento do drone 
      delay(500);
      if(f.ARMED){
        resetRcData();
      }
    }
    //teste para identificar comando de desligar
    else if(pulso == PULSO_DESLIGAR){
      rcData[THROTTLE] = 1000;
      rcData[YAW] = 1000;
    }
    else{
      //descompactaçao de pulso em dois comandos: UP e DOWN
      parametro1 = (pulso >> (sizeof(long)/2 * 8));
      parametro2  = ((pulso << (sizeof(long)/2 * 8)) >> (sizeof(long)/2 * 8));
      
      //passagem dos parametros para a funçao que comanda o drone
      controleDrone(parametro1, parametro2);
    }
  }
  rcs.resetAvailable();
}
